#include "bdc.h"
#include <stdio.h>  
#include <unistd.h>  

// 电机定义  
#define TARGET_POSITION 1000 // 目标位置  
#define MAX_SPEED 255        // 最大PWM信号  
#define MIN_SPEED 0          // 最小PWM信号  

typedef struct {  
    int position;    // 当前位置信息  
    int speed;       // 当前速度信息  
    int current;     // 当前电流信息  
} DC_Motor;  

// PID控制器结构体  
typedef struct {  
    float Kp;  
    float Ki;  
    float Kd;  
    float integral;  
    int previous_error;  
} PID_Controller;  

// 更新电机状态  
void update_motor(DC_Motor *motor, int control_signal) {  
    // 根据控制信号更新电流，随后更新速度与位置  
    motor->current = control_signal; // 电流控制信号直接作为PWM值  
    motor->speed += (motor->current - motor->speed) * 0.1; // 简单的速度更新  
    motor->position += motor->speed; // 简单的位移更新  
}  

// PID控制计算  
int PID_control(PID_Controller *pid, int target, int current) {  
    int error = target - current;  
    pid->integral += error;  
    int derivative = error - pid->previous_error;  
    
    // 上次误差更新  
    pid->previous_error = error;  

    // PID输出  
    return (int)(pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative);  
}  

int main() {  
    DC_Motor motor = {0, 0, 0}; // 初始化电机状态  
    PID_Controller position_pid = {1.0, 0.1, 0.01, 0, 0}; // 位置PID控制器  
    PID_Controller speed_pid = {1.0, 0.1, 0.01, 0, 0};   // 速度PID控制器  
    int control_signal;  

    // 模拟控制过程  
    for (int i = 0; i < 100; i++) {  
        // 位置控制  
        int p_control = PID_control(&position_pid, TARGET_POSITION, motor.position);  
        
        // 速度控制  
        int v_control = PID_control(&speed_pid, 0, motor.speed); // 目标速度设为0  

        // 简单的控制信号合成  
        control_signal = p_control + v_control;  

        // 限制控制信号在 PWM 范围内  
        if (control_signal > MAX_SPEED) control_signal = MAX_SPEED;  
        if (control_signal < MIN_SPEED) control_signal = MIN_SPEED;  

        // 更新电机状态  
        update_motor(&motor, control_signal);  

        // 打印当前状态  
        printf("Iteration: %d | Position: %d | Speed: %d | Current: %d | Control Signal: %d\n",   
               i, motor.position, motor.speed, motor.current, control_signal);  
        
        usleep(100000); // 模拟采样时间  
    }  

    return 0;  
}